// The original version was released under the following license
/**
* Copyright (C) 2014-2016 Ra�l Mur-Artal <raulmur at unizar dot es> (University of Zaragoza)
* For more information see <https://github.com/raulmur/ORB_SLAM2>
*
* ORB-SLAM2 is free software: you can redistribute it and/or modify
* it under the terms of the GNU General Public License as published by
* the Free Software Foundation, either version 3 of the License, or
* (at your option) any later version.
*
* ORB-SLAM2 is distributed in the hope that it will be useful,
* but WITHOUT ANY WARRANTY; without even the implied warranty of
* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
* GNU General Public License for more details.
*
* You should have received a copy of the GNU General Public License
* along with ORB-SLAM2. If not, see <http://www.gnu.org/licenses/>.
*/

// All modifications are released under the following license
/**
* This file is part of MultiCol-SLAM
*
* Copyright (C) 2015-2016 Steffen Urban <rurbste at googlemail.com>
* For more information see <https://github.com/urbste/MultiCol-SLAM>
*
* MultiCol-SLAM is free software: you can redistribute it and/or modify
* it under the terms of the GNU General Public License as published by
* the Free Software Foundation, either version 3 of the License, or
* (at your option) any later version.
*
* MultiCol-SLAM is distributed in the hope that it will be useful,
* but WITHOUT ANY WARRANTY; without even the implied warranty of
* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
* GNU General Public License for more details.
*
* You should have received a copy of the GNU General Public License
* along with MultiCol-SLAM . If not, see <http://www.gnu.org/licenses/>.
*/

#include <iostream>
#include <fstream> 
#include <iomanip>
#include <thread>
#include <mutex>

#include <sstream>
#include <fstream>
#include <filesystem>

#include <opencv2/core/core.hpp>

#include <vector>
#include <opencv2/opencv.hpp>

#include "cTracking.h"
#include "cConverter.h"
#include "cam_model_omni.h"
#include "cSystem.h"

using namespace std;


void LoadImagesAndTimestamps(
	const int startFrame,
	const int endFrame,
	const string path2imgs,
	vector<vector<string>> &vstrImageFilenames,
	vector<double> &vTimestamps);

int main(int argc, char **argv)
{
	if (argc != 5)
	{
		cerr << endl << "Usage: ./MultiCol_Slam_Lafida vocabulary_file slam_settings_file path_to_settings path_to_img_sequence" << endl;
		return 1;
	}

	string path2voc = string(argv[1]);
	string path2settings = string(argv[2]);
	string path2calibrations = string(argv[3]);
	string path2imgs = string(argv[4]);

	cout << endl << "MultiCol-SLAM Copyright (C) 2016 Steffen Urban" << endl << endl;
	// --------------
	// 1. Tracking settings
	// --------------
	cv::FileStorage frameSettings(path2settings, cv::FileStorage::READ);

	int traj = (int)frameSettings["traj2Eval"];
	string trajs = to_string(traj);
	const int endFrame = (int)frameSettings["traj.EndFrame"];
	const int startFrame = (int)frameSettings["traj.StartFrame"];

	// --------------
	// 4. Load image paths and timestamps 加载图像和时间戳
	// --------------
	vector<vector<string>> imgFilenames;
	vector<double> timestamps;
	LoadImagesAndTimestamps(startFrame, endFrame, path2imgs, imgFilenames, timestamps);

	int nImages = imgFilenames[0].size();

	cout << "nImages：" << nImages << endl;

	// for (int i = 1; i < timestamps.size(); i++) {
	// 	if (timestamps[i] != timestamps[i  -1])
	// 		cout << " true" <<endl;
	// 	// cout  << "timestamps[" << to_string(i) << "]：" << timestamps[i] << endl;
	// }

	// ////////////////////////////////////////////////
	
	cout << "嗨害嗨" << path2calibrations << endl;

	// ////////////////////////////////////////////////

	// 下面全都先不运行，先在上面的LoadImagesAndTimestamps中把数据读出来

	// ///////////////////////////////////////////////
	// 定义一个MultiColSLAM系统
	MultiColSLAM::cSystem MultiSLAM(path2voc, path2settings, path2calibrations, true);

	// Vector for tracking time statistics
	vector<float> vTimesTrack;
	vTimesTrack.resize(nImages);

	cout << endl << "-------" << endl;
	cout << "Start processing sequence ..." << endl;
	cout << "Images in the sequence: " << nImages << endl << endl;

	// Main loop
	const int nrCams = static_cast<int>(imgFilenames.size()); // 相机数量
	std::vector<cv::Mat> imgs(nrCams);  // 保存对应nrCams个相机的图像
	for (int ni = 0; ni < nImages; ni++)
	{
		// Read image from file
		std::vector<bool> loaded(nrCams); // 这个没用
		// 读取第 c 个相机的第 ni 张图像
		for (int c = 0; c < nrCams; ++c)
		{
			imgs[c] = cv::imread(imgFilenames[c][ni], CV_LOAD_IMAGE_GRAYSCALE);
			// imgs[c] = cv::imread(imgFilenames[c][ni], CV_LOAD_IMAGE_COLOR);
			if (imgs[c].empty())
			{
				cerr << endl << "Failed to load image at: " << imgFilenames[c][ni] << endl;
				return 1;
			}
		}
		// 当前时间戳
		double tframe = timestamps[ni];
		std::chrono::steady_clock::time_point t1 = std::chrono::steady_clock::now();
		// Pass the image to the SLAM system
		// 将图像传递给SLAM系统，进行跟踪
		MultiSLAM.TrackMultiColSLAM(imgs, tframe); // imgs：imgs[0]-imgs[2]  包括3个相机，在 tframe 时刻的图像集合

		std::chrono::steady_clock::time_point t2 = std::chrono::steady_clock::now();

		// 计算处理当前帧所花费的时间
		double ttrack = std::chrono::duration_cast<std::chrono::duration<double> >(t2 - t1).count();

		vTimesTrack[ni] = ttrack; // 将处理时间存储在向量 vTimesTrack 中

		// Wait to load the next frame
		// 等待加载下一帧
		double T = 0;  // 初始化等待时间
		if (ni < nImages - 1)
			T = (timestamps[ni + 1] - tframe) / 100;  // 如果不是最后一帧，则计算下一帧和当前帧之间的时间间隔
		else if (ni > 0)
			T = (tframe - timestamps[ni - 1]) / 100;  // 如果是最后一帧，则计算当前帧和前一帧之间的时间间隔
		//std::this_thread::sleep_for(std::chrono::milliseconds(30));
		// 输出等待时间

		// T = 300;  // 手动修改T值
		cout << "ttrack：" << ttrack << endl <<  "T：" <<  T << endl;
		

		if (ttrack < T)  // 如果处理时间小于等待时间
			std::this_thread::sleep_for(std::chrono::milliseconds(
			static_cast<long>((T - ttrack))));  // 等待剩余时间，以保持固定的帧率
	}

	// Stop all threads
	MultiSLAM.Shutdown();

	// Tracking time statistics
	sort(vTimesTrack.begin(), vTimesTrack.end());
	float totaltime = 0;
	for (int ni = 0; ni<nImages; ni++)
	{
		totaltime += vTimesTrack[ni];
	}
	cout << "-------" << endl << endl;
	cout << "median tracking time: " << vTimesTrack[nImages / 2] << endl;
	cout << "mean tracking time: " << totaltime / nImages << endl;

	// Save camera trajectory
	// 保存相机轨迹
	MultiSLAM.SaveMKFTrajectoryLAFIDA("MKFTrajectory.txt");

	return 0;
}

// ///////////////////////////////////////////////////////////////
// 
// 原始代码
// 
// //////////////////////////////////////////////////////////////
// void LoadImagesAndTimestamps(const int startFrame,
// 	const int endFrame,
// 	const string path2imgs,
// 	vector<vector<string>> &vstrImageFilenames,
// 	vector<double> &vTimestamps)
// { // 读取图像和对应的时间戳
// 	vstrImageFilenames.resize(3);
// 	ifstream fTimes;
// 	// old
// 	string strPathTimeFile = path2imgs + "/images_and_timestamps.txt";
// 	// cur
// 	// string strPathTimeFile = path2imgs + "/left_timestamps.txt";

// 	fTimes.open(strPathTimeFile.c_str());
// 	string line;

// 	cout << strPathTimeFile << endl;


// 	int cnt = 1;
// 	while (std::getline(fTimes, line))
// 	{
// 		if (cnt >= startFrame && cnt < endFrame) // skip until startframe
// 		{
// 			std::istringstream iss(line);
// 			double timestamp;
// 			string pathimg1, pathimg2, pathimg3;
// 			if (!(iss >> timestamp >> pathimg1 >> pathimg2 >> pathimg3))
// 				break;
// 			cout << path2imgs + '/' + pathimg1 << endl;
// 			vTimestamps.push_back(timestamp);
// 			vstrImageFilenames[0].push_back(path2imgs + '/' + pathimg1);
// 			vstrImageFilenames[1].push_back(path2imgs + '/' + pathimg2);
// 			vstrImageFilenames[2].push_back(path2imgs + '/' + pathimg3);

// 			// cout << vstrImageFilenames[0][1] << endl;

// 		}
// 		++cnt;

// 	}
// }

// //////////////////////////////////////////////////////////////////
// 重写
/**
 * strPathTimeFile：基准文件
 * path2imgs：图像路径
 * vTimestamps：时间戳数组
*/
void alignTimestamps(string strPathTimeFile, const string path2imgs, vector<double> &vTimestamps) {
	cout << strPathTimeFile << " " << path2imgs << endl;
	ifstream fTimes;
	fTimes.open(strPathTimeFile.c_str());
	string line;

	while (std::getline(fTimes, line)) {
		std::istringstream iss(line);
		double timestamp;
		string pathimg1;

		if (!(iss >> timestamp >> pathimg1))
			break;
		vTimestamps.push_back((long long)timestamp);  // 时间戳数组 
		// stringstream ss;
		// string str_timestamp;
		// ss << (long long)timestamp;
		// ss >> str_timestamp;

		// cout << str_timestamp << endl;
	}

	string oldRearPath = path2imgs + "/imgs/cam0/*.png"; // rear
	string oldRightPath = path2imgs + "/imgs/cam1/*.png"; // right
	// string oldLeftPath = path2imgs + "/imgs/cam2/*.png"; // left
	// string newRearPath = path2imgs + "/imgs/rear";
	// string newRightPath = path2imgs + "/imgs/right";
	vector<cv::String> result;

	cout << "vTimestamps.size()：" << vTimestamps.size() << endl;
	for (const auto& str : vTimestamps) {
		cout << (long long)str << endl;
	}

	// 获取待修改  保存在result中
	cv::glob(oldRearPath, result);

	int index = 0;
	int cnt = 0;
	for (const auto& res : result) {
		std::string temp = res; // cv::String  -> std::string  old
		stringstream ss;
		string str_timestamp;
		ss << (long long)vTimestamps[index++];
		ss >> str_timestamp;

		// cout << "temp: " << temp << endl;
		
		if (temp != path2imgs + "/imgs/cam0/" +str_timestamp + ".png") {  // old != new  rear(和right)里面有和left不匹配的图片
			// 在这里进行修改  可以不打开文件

			// cout << index << "  DIFF" << endl;
			// cnt++;
			// continue;

			fstream f;
			f.open(temp.c_str());

			if (f.fail()) {
				cout << "File Open Failed!" << endl;
				f.close();
			} else {
				f.close();
				string newRearName = path2imgs + "/imgs/cam0/" +str_timestamp + ".png"; // rear的新名字
				// string newRightName = path2imgs + "/imgs/cam0/" +str_timestamp + ".png";
				// cv::Mat image = cv::imread(temp); // old 
				// cv::imwrite(newName, image); // new
				// ......................................................

				if (rename(temp.c_str(), newRearName.c_str()) == -1) {
					cout << "File rename failed!" << endl;
				} else {
					cout << "第 " << index << " 个 rear 被修改了" << endl;
					cnt++;
				}

			}
		}
	}
	cout << "cnt: " << cnt << endl;

	// ////////////////////////////////////////////////////////
	// 右相机

	// 获取待修改  保存在result中
	result.clear();
	cv::glob(oldRightPath, result);

	index = 0;
	cnt = 0;
	for (const auto& res : result) {
		std::string temp = res; // cv::String  -> std::string  old
		stringstream ss;
		string str_timestamp;
		ss << (long long)vTimestamps[index++];
		ss >> str_timestamp;

		// cout << "temp: " << temp << endl;
		
		if (temp != path2imgs + "/imgs/cam1/" +str_timestamp + ".png") {  // old != new  right(和rear)里面有和left不匹配的图片
			// 在这里进行修改  可以不打开文件

			// cout << index << "  DIFF" << endl;
			// cnt++;
			// continue;

			fstream f;
			f.open(temp.c_str());

			if (f.fail()) {
				cout << "File Open Failed!" << endl;
				f.close();
			} else {
				f.close();
				// string newRearName = path2imgs + "/imgs/cam0/" +str_timestamp + ".png"; // rear的新名字
				string newRightName = path2imgs + "/imgs/cam1/" +str_timestamp + ".png";
				// cv::Mat image = cv::imread(temp); // old 
				// cv::imwrite(newName, image); // new
				// ......................................................

				if (rename(temp.c_str(), newRightName.c_str()) == -1) {
					cout << "File rename failed!" << endl;
				} else {
					cout << "第 " << index << " 个 right 被修改了" << endl;
					cnt++;
				}

			}
		}
		
	}
	cout << "cnt: " << cnt << endl;
	
}



/**
 * 重写——适配robotCar数据集
 * 
 * startFrame： 开始帧
 * endFrame：结束帧
 * path2imgs：图像所在目录  /home/zc/slam/data/robotCarDataset/mono
 * vstrImageFilenames：存储图像名称
 * vTimestamps：存储时间戳
 * 
*/
// //////////////////////////////////////////////////////////////////
void LoadImagesAndTimestamps(const int startFrame,
	const int endFrame,
	const string path2imgs,
	vector<vector<string>> &vstrImageFilenames,
	vector<double> &vTimestamps)
{ // 读取图像和对应的时间戳
	vstrImageFilenames.resize(3);
	ifstream fTimes;
	string strPathTimeFile = path2imgs + "/left_timestamps.txt"; // 左相机时间戳文件
	// string strPathTimeFile = path2imgs + "/images_and_timestamps.txt"; // 左相机时间戳文件

	// 时间戳对齐函数
	// alignTimestamps(strPathTimeFile, path2imgs, vTimestamps); // 时间戳对齐

	string rearFile = path2imgs + "/imgs/cam0/"; // rear
	string rightFile = path2imgs + "/imgs/cam1/"; // right
	
	// cout << "path2imgs：" << path2imgs <<endl;

	fTimes.open(strPathTimeFile.c_str());
	string line;

	int cnt = 1;
	vTimestamps.clear();

	while (std::getline(fTimes, line))
	{
		if (cnt >= startFrame && cnt < endFrame) // skip until startframe
		{
			std::istringstream iss(line);
			double timestamp;
			string pathimg1, pathimg2, pathimg3;

			if (!(iss >> timestamp >> pathimg1))
				break;
			vTimestamps.push_back((long long)timestamp);
			// vTimestamps.push_back(timestamp);
			
			// 时间戳不一样
			// 如果时间戳不一样，把不一样的图像的时间戳换成现在的left基准的时间戳，然后加入到vstrImageFilenames中

			//  时间戳double->string
			stringstream ss;
			string str_timestamp;
			ss << (long long)timestamp;
			ss >> str_timestamp;
			// 1417794166777396.png

			// cout << "rearFile：" << rearFile +  str_timestamp + ".png" << endl;

			vstrImageFilenames[0].push_back(rearFile +  str_timestamp + ".png") ; // rear
			vstrImageFilenames[1].push_back(rightFile + str_timestamp + ".png"); // right
			vstrImageFilenames[2].push_back(path2imgs + "/imgs/cam2/" +  str_timestamp + ".png"); // left

			// test single img   
			// vstrImageFilenames[0].push_back(rearFile + "1417794166777396.png"); // rear
			// vstrImageFilenames[1].push_back(rightFile + "1417794166777396.png"); // right
			// vstrImageFilenames[2].push_back(path2imgs + "/imgs/cam2/1417794166777396.png"); // left


			// cout << (long long)timestamp << endl;
		}
		++cnt;

	}
}


// 下午：重新运行build.sh  判断vTimestamps里面的数据是什么